
// import haxe.ds.Array;
// import oimo.m.Mat3;
// import oimo.m.M;
// import oimo.dynamics.constraint.info.JacobianRow;
// import oimo.dynamics.constraint.info.joint.JointSolverInfo;
// import oimo.dynamics.constraint.solver.common.JointSolverMassDataRow;
// import oimo.dynamics.rigidbody.RigidBody;

import { JointSolverMassDataRow } from "../common/JointSolverMassDataRow";
import { JointSolverInfo } from "../../info/joint/JointSolverInfo";
import { RigidBody } from "../../../rigidbody/RigidBody";
import { Mat3 } from "../../../../common/Mat3";
import { M } from "../../../../common/M";
import { JacobianRow } from "../../info/JacobianRow";

/**
 * numberernal class
 */


export class MassMatrix {
	// matrix size
	_size: number;
	// inverse matrix elements
	_invMass: Array<Array<number>>;
	_invMassWithoutCfm: Array<Array<number>>;

	_massData: Array<JointSolverMassDataRow>;

	_cachedSubmatrices: Array<Array<Array<number>>>;
	_cacheComputed: Array<boolean>;
	_maxSubmatrixId: number;

	// temp matrix used for computing a inverse matrix
	tmpMatrix: Array<Array<number>>;

	constructor(size: number) {
		this._size = size;

		this.tmpMatrix = new Array<Array<number>>(this._size);
		this._invMass = new Array<Array<number>>(this._size);
		this._invMassWithoutCfm = new Array<Array<number>>(this._size);
		for (let i = 0; i < this._size; ++i) {
			this.tmpMatrix[i] = new Array<number>(this._size);
			this._invMass[i] = new Array<number>(this._size);
			this._invMassWithoutCfm[i] = new Array<number>(this._size);
			for (let j = 0; j < this._size; ++j) {
				this.tmpMatrix[i][j] = 0;
				this._invMass[i][j] = 0;
				this._invMassWithoutCfm[i][j] = 0;
			}
		}
		this._maxSubmatrixId = 1 << this._size;
		this._cacheComputed = new Array<boolean>(this._maxSubmatrixId);
		this._cachedSubmatrices = new Array<Array<Array<number>>>(this._maxSubmatrixId);
		for (let i = 0; i < this._maxSubmatrixId; ++i) {
			// popcount (assuming the size of the matrix is less than 0x100 = 256)
			var t: number = i;
			t = (t & 0x55) + (t >> 1 & 0x55);
			t = (t & 0x33) + (t >> 2 & 0x33);
			t = (t & 0xf) + (t >> 4 & 0xf);
			var matrixSize: number = t;

			var subMatrix: Array<Array<number>> = new Array<Array<number>>(matrixSize);
			for (let j = 0; j < matrixSize; ++j) {
				subMatrix[j] = new Array<number>(matrixSize);
				for (let k = 0; k < matrixSize; ++k) {
					subMatrix[j][k] = 0;
				}
			}
			this._cacheComputed[i] = false;
			this._cachedSubmatrices[i] = subMatrix;
		}
	}

	// --- ---

	public computeSubmatrix(id: number, indices: Array<number>, size: number): void {
		// compute the inverse matrix of the submatrix of the inverse mass matrix
		//
		//      | invData[i[0]][i[0]]    ...  invData[i[0]][i[n-1]]   |
		// inv( | ...                    ...  ...                     | )
		//      | invData[i[n-1]][i[0]]  ...  invData[i[n-1]][i[n-1]] |

		for (let i = 0; i < size; ++i) {
			var ii: number = indices[i];
			for (let j = 0; j < size; ++j) {
				this.tmpMatrix[i][j] = this._invMass[ii][indices[j]];
			}
		}

		var src: Array<Array<number>> = this.tmpMatrix;
		var dst: Array<Array<number>> = this._cachedSubmatrices[id];
		var srci: Array<number>;
		var dsti: Array<number>;
		var srcj: Array<number>;
		var dstj: Array<number>;
		var diag: number;

		switch (size) {
			case 4:
				srci = src[0]; dsti = dst[0]; diag = 1 / srci[0]; dsti[0] = diag; srci[1] = srci[1] * diag; srci[2] = srci[2] * diag; srci[3] = srci[3] * diag; srcj = src[1]; dstj = dst[1]; dstj[0] = -diag * srcj[0]; srcj[1] = srcj[1] - srci[1] * srcj[0]; srcj[2] = srcj[2] - srci[2] * srcj[0]; srcj[3] = srcj[3] - srci[3] * srcj[0]; srcj = src[2]; dstj = dst[2]; dstj[0] = -diag * srcj[0]; srcj[1] = srcj[1] - srci[1] * srcj[0]; srcj[2] = srcj[2] - srci[2] * srcj[0]; srcj[3] = srcj[3] - srci[3] * srcj[0]; srcj = src[3]; dstj = dst[3]; dstj[0] = -diag * srcj[0]; srcj[1] = srcj[1] - srci[1] * srcj[0]; srcj[2] = srcj[2] - srci[2] * srcj[0]; srcj[3] = srcj[3] - srci[3] * srcj[0]; srci = src[1]; dsti = dst[1]; diag = 1 / srci[1]; dsti[1] = diag; dsti[0] = dsti[0] * diag; srci[2] = srci[2] * diag; srci[3] = srci[3] * diag; srcj = src[0]; dstj = dst[0]; dstj[0] = dstj[0] - dsti[0] * srcj[1]; srcj[2] = srcj[2] - srci[2] * srcj[1]; srcj[3] = srcj[3] - srci[3] * srcj[1]; srcj = src[2]; dstj = dst[2]; dstj[0] = dstj[0] - dsti[0] * srcj[1]; dstj[1] = -diag * srcj[1]; srcj[2] = srcj[2] - srci[2] * srcj[1]; srcj[3] = srcj[3] - srci[3] * srcj[1]; srcj = src[3]; dstj = dst[3]; dstj[0] = dstj[0] - dsti[0] * srcj[1]; dstj[1] = -diag * srcj[1]; srcj[2] = srcj[2] - srci[2] * srcj[1]; srcj[3] = srcj[3] - srci[3] * srcj[1]; srci = src[2]; dsti = dst[2]; diag = 1 / srci[2]; dsti[2] = diag; dsti[0] = dsti[0] * diag; dsti[1] = dsti[1] * diag; srci[3] = srci[3] * diag; srcj = src[0]; dstj = dst[0]; dstj[0] = dstj[0] - dsti[0] * srcj[2]; srcj[3] = srcj[3] - srci[3] * srcj[2]; srcj = src[1]; dstj = dst[1]; dstj[0] = dstj[0] - dsti[0] * srcj[2]; dstj[1] = dstj[1] - dsti[1] * srcj[2]; srcj[3] = srcj[3] - srci[3] * srcj[2]; srcj = src[3]; dstj = dst[3]; dstj[0] = dstj[0] - dsti[0] * srcj[2]; dstj[1] = dstj[1] - dsti[1] * srcj[2]; dstj[2] = -diag * srcj[2]; srcj[3] = srcj[3] - srci[3] * srcj[2]; srci = src[3]; dsti = dst[3]; diag = 1 / srci[3]; dsti[3] = diag; dsti[0] = dsti[0] * diag; dsti[1] = dsti[1] * diag; dsti[2] = dsti[2] * diag; srcj = src[0]; dstj = dst[0]; dstj[0] = dstj[0] - dsti[0] * srcj[3]; srcj = src[1]; dstj = dst[1]; dstj[0] = dstj[0] - dsti[0] * srcj[3]; dstj[1] = dstj[1] - dsti[1] * srcj[3]; srcj = src[2]; dstj = dst[2]; dstj[0] = dstj[0] - dsti[0] * srcj[3]; dstj[1] = dstj[1] - dsti[1] * srcj[3]; dstj[2] = dstj[2] - dsti[2] * srcj[3]; dsti = dst[1]; dst[0][1] = dsti[0]; dsti = dst[2]; dst[0][2] = dsti[0]; dst[1][2] = dsti[1]; dsti = dst[3]; dst[0][3] = dsti[0]; dst[1][3] = dsti[1]; dst[2][3] = dsti[2];
				break;
			case 5:
				srci = src[0]; dsti = dst[0]; diag = 1 / srci[0]; dsti[0] = diag; srci[1] = srci[1] * diag; srci[2] = srci[2] * diag; srci[3] = srci[3] * diag; srci[4] = srci[4] * diag; srcj = src[1]; dstj = dst[1]; dstj[0] = -diag * srcj[0]; srcj[1] = srcj[1] - srci[1] * srcj[0]; srcj[2] = srcj[2] - srci[2] * srcj[0]; srcj[3] = srcj[3] - srci[3] * srcj[0]; srcj[4] = srcj[4] - srci[4] * srcj[0]; srcj = src[2]; dstj = dst[2]; dstj[0] = -diag * srcj[0]; srcj[1] = srcj[1] - srci[1] * srcj[0]; srcj[2] = srcj[2] - srci[2] * srcj[0]; srcj[3] = srcj[3] - srci[3] * srcj[0]; srcj[4] = srcj[4] - srci[4] * srcj[0]; srcj = src[3]; dstj = dst[3]; dstj[0] = -diag * srcj[0]; srcj[1] = srcj[1] - srci[1] * srcj[0]; srcj[2] = srcj[2] - srci[2] * srcj[0]; srcj[3] = srcj[3] - srci[3] * srcj[0]; srcj[4] = srcj[4] - srci[4] * srcj[0]; srcj = src[4]; dstj = dst[4]; dstj[0] = -diag * srcj[0]; srcj[1] = srcj[1] - srci[1] * srcj[0]; srcj[2] = srcj[2] - srci[2] * srcj[0]; srcj[3] = srcj[3] - srci[3] * srcj[0]; srcj[4] = srcj[4] - srci[4] * srcj[0]; srci = src[1]; dsti = dst[1]; diag = 1 / srci[1]; dsti[1] = diag; dsti[0] = dsti[0] * diag; srci[2] = srci[2] * diag; srci[3] = srci[3] * diag; srci[4] = srci[4] * diag; srcj = src[0]; dstj = dst[0]; dstj[0] = dstj[0] - dsti[0] * srcj[1]; srcj[2] = srcj[2] - srci[2] * srcj[1]; srcj[3] = srcj[3] - srci[3] * srcj[1]; srcj[4] = srcj[4] - srci[4] * srcj[1]; srcj = src[2]; dstj = dst[2]; dstj[0] = dstj[0] - dsti[0] * srcj[1]; dstj[1] = -diag * srcj[1]; srcj[2] = srcj[2] - srci[2] * srcj[1]; srcj[3] = srcj[3] - srci[3] * srcj[1]; srcj[4] = srcj[4] - srci[4] * srcj[1]; srcj = src[3]; dstj = dst[3]; dstj[0] = dstj[0] - dsti[0] * srcj[1]; dstj[1] = -diag * srcj[1]; srcj[2] = srcj[2] - srci[2] * srcj[1]; srcj[3] = srcj[3] - srci[3] * srcj[1]; srcj[4] = srcj[4] - srci[4] * srcj[1]; srcj = src[4]; dstj = dst[4]; dstj[0] = dstj[0] - dsti[0] * srcj[1]; dstj[1] = -diag * srcj[1]; srcj[2] = srcj[2] - srci[2] * srcj[1]; srcj[3] = srcj[3] - srci[3] * srcj[1]; srcj[4] = srcj[4] - srci[4] * srcj[1]; srci = src[2]; dsti = dst[2]; diag = 1 / srci[2]; dsti[2] = diag; dsti[0] = dsti[0] * diag; dsti[1] = dsti[1] * diag; srci[3] = srci[3] * diag; srci[4] = srci[4] * diag; srcj = src[0]; dstj = dst[0]; dstj[0] = dstj[0] - dsti[0] * srcj[2]; srcj[3] = srcj[3] - srci[3] * srcj[2]; srcj[4] = srcj[4] - srci[4] * srcj[2]; srcj = src[1]; dstj = dst[1]; dstj[0] = dstj[0] - dsti[0] * srcj[2]; dstj[1] = dstj[1] - dsti[1] * srcj[2]; srcj[3] = srcj[3] - srci[3] * srcj[2]; srcj[4] = srcj[4] - srci[4] * srcj[2]; srcj = src[3]; dstj = dst[3]; dstj[0] = dstj[0] - dsti[0] * srcj[2]; dstj[1] = dstj[1] - dsti[1] * srcj[2]; dstj[2] = -diag * srcj[2]; srcj[3] = srcj[3] - srci[3] * srcj[2]; srcj[4] = srcj[4] - srci[4] * srcj[2]; srcj = src[4]; dstj = dst[4]; dstj[0] = dstj[0] - dsti[0] * srcj[2]; dstj[1] = dstj[1] - dsti[1] * srcj[2]; dstj[2] = -diag * srcj[2]; srcj[3] = srcj[3] - srci[3] * srcj[2]; srcj[4] = srcj[4] - srci[4] * srcj[2]; srci = src[3]; dsti = dst[3]; diag = 1 / srci[3]; dsti[3] = diag; dsti[0] = dsti[0] * diag; dsti[1] = dsti[1] * diag; dsti[2] = dsti[2] * diag; srci[4] = srci[4] * diag; srcj = src[0]; dstj = dst[0]; dstj[0] = dstj[0] - dsti[0] * srcj[3]; srcj[4] = srcj[4] - srci[4] * srcj[3]; srcj = src[1]; dstj = dst[1]; dstj[0] = dstj[0] - dsti[0] * srcj[3]; dstj[1] = dstj[1] - dsti[1] * srcj[3]; srcj[4] = srcj[4] - srci[4] * srcj[3]; srcj = src[2]; dstj = dst[2]; dstj[0] = dstj[0] - dsti[0] * srcj[3]; dstj[1] = dstj[1] - dsti[1] * srcj[3]; dstj[2] = dstj[2] - dsti[2] * srcj[3]; srcj[4] = srcj[4] - srci[4] * srcj[3]; srcj = src[4]; dstj = dst[4]; dstj[0] = dstj[0] - dsti[0] * srcj[3]; dstj[1] = dstj[1] - dsti[1] * srcj[3]; dstj[2] = dstj[2] - dsti[2] * srcj[3]; dstj[3] = -diag * srcj[3]; srcj[4] = srcj[4] - srci[4] * srcj[3]; srci = src[4]; dsti = dst[4]; diag = 1 / srci[4]; dsti[4] = diag; dsti[0] = dsti[0] * diag; dsti[1] = dsti[1] * diag; dsti[2] = dsti[2] * diag; dsti[3] = dsti[3] * diag; srcj = src[0]; dstj = dst[0]; dstj[0] = dstj[0] - dsti[0] * srcj[4]; srcj = src[1]; dstj = dst[1]; dstj[0] = dstj[0] - dsti[0] * srcj[4]; dstj[1] = dstj[1] - dsti[1] * srcj[4]; srcj = src[2]; dstj = dst[2]; dstj[0] = dstj[0] - dsti[0] * srcj[4]; dstj[1] = dstj[1] - dsti[1] * srcj[4]; dstj[2] = dstj[2] - dsti[2] * srcj[4]; srcj = src[3]; dstj = dst[3]; dstj[0] = dstj[0] - dsti[0] * srcj[4]; dstj[1] = dstj[1] - dsti[1] * srcj[4]; dstj[2] = dstj[2] - dsti[2] * srcj[4]; dstj[3] = dstj[3] - dsti[3] * srcj[4]; dsti = dst[1]; dst[0][1] = dsti[0]; dsti = dst[2]; dst[0][2] = dsti[0]; dst[1][2] = dsti[1]; dsti = dst[3]; dst[0][3] = dsti[0]; dst[1][3] = dsti[1]; dst[2][3] = dsti[2]; dsti = dst[4]; dst[0][4] = dsti[0]; dst[1][4] = dsti[1]; dst[2][4] = dsti[2]; dst[3][4] = dsti[3];
				break;
			case 6:
				srci = src[0]; dsti = dst[0]; diag = 1 / srci[0]; dsti[0] = diag; srci[1] = srci[1] * diag; srci[2] = srci[2] * diag; srci[3] = srci[3] * diag; srci[4] = srci[4] * diag; srci[5] = srci[5] * diag; srcj = src[1]; dstj = dst[1]; dstj[0] = -diag * srcj[0]; srcj[1] = srcj[1] - srci[1] * srcj[0]; srcj[2] = srcj[2] - srci[2] * srcj[0]; srcj[3] = srcj[3] - srci[3] * srcj[0]; srcj[4] = srcj[4] - srci[4] * srcj[0]; srcj[5] = srcj[5] - srci[5] * srcj[0]; srcj = src[2]; dstj = dst[2]; dstj[0] = -diag * srcj[0]; srcj[1] = srcj[1] - srci[1] * srcj[0]; srcj[2] = srcj[2] - srci[2] * srcj[0]; srcj[3] = srcj[3] - srci[3] * srcj[0]; srcj[4] = srcj[4] - srci[4] * srcj[0]; srcj[5] = srcj[5] - srci[5] * srcj[0]; srcj = src[3]; dstj = dst[3]; dstj[0] = -diag * srcj[0]; srcj[1] = srcj[1] - srci[1] * srcj[0]; srcj[2] = srcj[2] - srci[2] * srcj[0]; srcj[3] = srcj[3] - srci[3] * srcj[0]; srcj[4] = srcj[4] - srci[4] * srcj[0]; srcj[5] = srcj[5] - srci[5] * srcj[0]; srcj = src[4]; dstj = dst[4]; dstj[0] = -diag * srcj[0]; srcj[1] = srcj[1] - srci[1] * srcj[0]; srcj[2] = srcj[2] - srci[2] * srcj[0]; srcj[3] = srcj[3] - srci[3] * srcj[0]; srcj[4] = srcj[4] - srci[4] * srcj[0]; srcj[5] = srcj[5] - srci[5] * srcj[0]; srcj = src[5]; dstj = dst[5]; dstj[0] = -diag * srcj[0]; srcj[1] = srcj[1] - srci[1] * srcj[0]; srcj[2] = srcj[2] - srci[2] * srcj[0]; srcj[3] = srcj[3] - srci[3] * srcj[0]; srcj[4] = srcj[4] - srci[4] * srcj[0]; srcj[5] = srcj[5] - srci[5] * srcj[0]; srci = src[1]; dsti = dst[1]; diag = 1 / srci[1]; dsti[1] = diag; dsti[0] = dsti[0] * diag; srci[2] = srci[2] * diag; srci[3] = srci[3] * diag; srci[4] = srci[4] * diag; srci[5] = srci[5] * diag; srcj = src[0]; dstj = dst[0]; dstj[0] = dstj[0] - dsti[0] * srcj[1]; srcj[2] = srcj[2] - srci[2] * srcj[1]; srcj[3] = srcj[3] - srci[3] * srcj[1]; srcj[4] = srcj[4] - srci[4] * srcj[1]; srcj[5] = srcj[5] - srci[5] * srcj[1]; srcj = src[2]; dstj = dst[2]; dstj[0] = dstj[0] - dsti[0] * srcj[1]; dstj[1] = -diag * srcj[1]; srcj[2] = srcj[2] - srci[2] * srcj[1]; srcj[3] = srcj[3] - srci[3] * srcj[1]; srcj[4] = srcj[4] - srci[4] * srcj[1]; srcj[5] = srcj[5] - srci[5] * srcj[1]; srcj = src[3]; dstj = dst[3]; dstj[0] = dstj[0] - dsti[0] * srcj[1]; dstj[1] = -diag * srcj[1]; srcj[2] = srcj[2] - srci[2] * srcj[1]; srcj[3] = srcj[3] - srci[3] * srcj[1]; srcj[4] = srcj[4] - srci[4] * srcj[1]; srcj[5] = srcj[5] - srci[5] * srcj[1]; srcj = src[4]; dstj = dst[4]; dstj[0] = dstj[0] - dsti[0] * srcj[1]; dstj[1] = -diag * srcj[1]; srcj[2] = srcj[2] - srci[2] * srcj[1]; srcj[3] = srcj[3] - srci[3] * srcj[1]; srcj[4] = srcj[4] - srci[4] * srcj[1]; srcj[5] = srcj[5] - srci[5] * srcj[1]; srcj = src[5]; dstj = dst[5]; dstj[0] = dstj[0] - dsti[0] * srcj[1]; dstj[1] = -diag * srcj[1]; srcj[2] = srcj[2] - srci[2] * srcj[1]; srcj[3] = srcj[3] - srci[3] * srcj[1]; srcj[4] = srcj[4] - srci[4] * srcj[1]; srcj[5] = srcj[5] - srci[5] * srcj[1]; srci = src[2]; dsti = dst[2]; diag = 1 / srci[2]; dsti[2] = diag; dsti[0] = dsti[0] * diag; dsti[1] = dsti[1] * diag; srci[3] = srci[3] * diag; srci[4] = srci[4] * diag; srci[5] = srci[5] * diag; srcj = src[0]; dstj = dst[0]; dstj[0] = dstj[0] - dsti[0] * srcj[2]; srcj[3] = srcj[3] - srci[3] * srcj[2]; srcj[4] = srcj[4] - srci[4] * srcj[2]; srcj[5] = srcj[5] - srci[5] * srcj[2]; srcj = src[1]; dstj = dst[1]; dstj[0] = dstj[0] - dsti[0] * srcj[2]; dstj[1] = dstj[1] - dsti[1] * srcj[2]; srcj[3] = srcj[3] - srci[3] * srcj[2]; srcj[4] = srcj[4] - srci[4] * srcj[2]; srcj[5] = srcj[5] - srci[5] * srcj[2]; srcj = src[3]; dstj = dst[3]; dstj[0] = dstj[0] - dsti[0] * srcj[2]; dstj[1] = dstj[1] - dsti[1] * srcj[2]; dstj[2] = -diag * srcj[2]; srcj[3] = srcj[3] - srci[3] * srcj[2]; srcj[4] = srcj[4] - srci[4] * srcj[2]; srcj[5] = srcj[5] - srci[5] * srcj[2]; srcj = src[4]; dstj = dst[4]; dstj[0] = dstj[0] - dsti[0] * srcj[2]; dstj[1] = dstj[1] - dsti[1] * srcj[2]; dstj[2] = -diag * srcj[2]; srcj[3] = srcj[3] - srci[3] * srcj[2]; srcj[4] = srcj[4] - srci[4] * srcj[2]; srcj[5] = srcj[5] - srci[5] * srcj[2]; srcj = src[5]; dstj = dst[5]; dstj[0] = dstj[0] - dsti[0] * srcj[2]; dstj[1] = dstj[1] - dsti[1] * srcj[2]; dstj[2] = -diag * srcj[2]; srcj[3] = srcj[3] - srci[3] * srcj[2]; srcj[4] = srcj[4] - srci[4] * srcj[2]; srcj[5] = srcj[5] - srci[5] * srcj[2]; srci = src[3]; dsti = dst[3]; diag = 1 / srci[3]; dsti[3] = diag; dsti[0] = dsti[0] * diag; dsti[1] = dsti[1] * diag; dsti[2] = dsti[2] * diag; srci[4] = srci[4] * diag; srci[5] = srci[5] * diag; srcj = src[0]; dstj = dst[0]; dstj[0] = dstj[0] - dsti[0] * srcj[3]; srcj[4] = srcj[4] - srci[4] * srcj[3]; srcj[5] = srcj[5] - srci[5] * srcj[3]; srcj = src[1]; dstj = dst[1]; dstj[0] = dstj[0] - dsti[0] * srcj[3]; dstj[1] = dstj[1] - dsti[1] * srcj[3]; srcj[4] = srcj[4] - srci[4] * srcj[3]; srcj[5] = srcj[5] - srci[5] * srcj[3]; srcj = src[2]; dstj = dst[2]; dstj[0] = dstj[0] - dsti[0] * srcj[3]; dstj[1] = dstj[1] - dsti[1] * srcj[3]; dstj[2] = dstj[2] - dsti[2] * srcj[3]; srcj[4] = srcj[4] - srci[4] * srcj[3]; srcj[5] = srcj[5] - srci[5] * srcj[3]; srcj = src[4]; dstj = dst[4]; dstj[0] = dstj[0] - dsti[0] * srcj[3]; dstj[1] = dstj[1] - dsti[1] * srcj[3]; dstj[2] = dstj[2] - dsti[2] * srcj[3]; dstj[3] = -diag * srcj[3]; srcj[4] = srcj[4] - srci[4] * srcj[3]; srcj[5] = srcj[5] - srci[5] * srcj[3]; srcj = src[5]; dstj = dst[5]; dstj[0] = dstj[0] - dsti[0] * srcj[3]; dstj[1] = dstj[1] - dsti[1] * srcj[3]; dstj[2] = dstj[2] - dsti[2] * srcj[3]; dstj[3] = -diag * srcj[3]; srcj[4] = srcj[4] - srci[4] * srcj[3]; srcj[5] = srcj[5] - srci[5] * srcj[3]; srci = src[4]; dsti = dst[4]; diag = 1 / srci[4]; dsti[4] = diag; dsti[0] = dsti[0] * diag; dsti[1] = dsti[1] * diag; dsti[2] = dsti[2] * diag; dsti[3] = dsti[3] * diag; srci[5] = srci[5] * diag; srcj = src[0]; dstj = dst[0]; dstj[0] = dstj[0] - dsti[0] * srcj[4]; srcj[5] = srcj[5] - srci[5] * srcj[4]; srcj = src[1]; dstj = dst[1]; dstj[0] = dstj[0] - dsti[0] * srcj[4]; dstj[1] = dstj[1] - dsti[1] * srcj[4]; srcj[5] = srcj[5] - srci[5] * srcj[4]; srcj = src[2]; dstj = dst[2]; dstj[0] = dstj[0] - dsti[0] * srcj[4]; dstj[1] = dstj[1] - dsti[1] * srcj[4]; dstj[2] = dstj[2] - dsti[2] * srcj[4]; srcj[5] = srcj[5] - srci[5] * srcj[4]; srcj = src[3]; dstj = dst[3]; dstj[0] = dstj[0] - dsti[0] * srcj[4]; dstj[1] = dstj[1] - dsti[1] * srcj[4]; dstj[2] = dstj[2] - dsti[2] * srcj[4]; dstj[3] = dstj[3] - dsti[3] * srcj[4]; srcj[5] = srcj[5] - srci[5] * srcj[4]; srcj = src[5]; dstj = dst[5]; dstj[0] = dstj[0] - dsti[0] * srcj[4]; dstj[1] = dstj[1] - dsti[1] * srcj[4]; dstj[2] = dstj[2] - dsti[2] * srcj[4]; dstj[3] = dstj[3] - dsti[3] * srcj[4]; dstj[4] = -diag * srcj[4]; srcj[5] = srcj[5] - srci[5] * srcj[4]; srci = src[5]; dsti = dst[5]; diag = 1 / srci[5]; dsti[5] = diag; dsti[0] = dsti[0] * diag; dsti[1] = dsti[1] * diag; dsti[2] = dsti[2] * diag; dsti[3] = dsti[3] * diag; dsti[4] = dsti[4] * diag; srcj = src[0]; dstj = dst[0]; dstj[0] = dstj[0] - dsti[0] * srcj[5]; srcj = src[1]; dstj = dst[1]; dstj[0] = dstj[0] - dsti[0] * srcj[5]; dstj[1] = dstj[1] - dsti[1] * srcj[5]; srcj = src[2]; dstj = dst[2]; dstj[0] = dstj[0] - dsti[0] * srcj[5]; dstj[1] = dstj[1] - dsti[1] * srcj[5]; dstj[2] = dstj[2] - dsti[2] * srcj[5]; srcj = src[3]; dstj = dst[3]; dstj[0] = dstj[0] - dsti[0] * srcj[5]; dstj[1] = dstj[1] - dsti[1] * srcj[5]; dstj[2] = dstj[2] - dsti[2] * srcj[5]; dstj[3] = dstj[3] - dsti[3] * srcj[5]; srcj = src[4]; dstj = dst[4]; dstj[0] = dstj[0] - dsti[0] * srcj[5]; dstj[1] = dstj[1] - dsti[1] * srcj[5]; dstj[2] = dstj[2] - dsti[2] * srcj[5]; dstj[3] = dstj[3] - dsti[3] * srcj[5]; dstj[4] = dstj[4] - dsti[4] * srcj[5]; dsti = dst[1]; dst[0][1] = dsti[0]; dsti = dst[2]; dst[0][2] = dsti[0]; dst[1][2] = dsti[1]; dsti = dst[3]; dst[0][3] = dsti[0]; dst[1][3] = dsti[1]; dst[2][3] = dsti[2]; dsti = dst[4]; dst[0][4] = dsti[0]; dst[1][4] = dsti[1]; dst[2][4] = dsti[2]; dst[3][4] = dsti[3]; dsti = dst[5]; dst[0][5] = dsti[0]; dst[1][5] = dsti[1]; dst[2][5] = dsti[2]; dst[3][5] = dsti[3]; dst[4][5] = dsti[4];
				break;
			default:
				for (let i = 0; i < size; ++i) {
					srci = src[i];
					dsti = dst[i];
					var diag: number = 1 / srci[i];
					dsti[i] = diag;
					for (let j = 0; j < i; ++j) {
						dsti[j] = dsti[j] * diag;
					}
					for (let j = i + 1; j < size; ++j) {
						srci[j] = srci[j] * diag;
					}
					for (let j = 0; j < i; ++j) {
						srcj = src[j];
						dstj = dst[j];
						for (let k = 0; k < j + 1; ++k) {
							dstj[k] = dstj[k] - dsti[k] * srcj[i];
						}
						for (let k = i + 1; k < size; ++k) {
							srcj[k] = srcj[k] - srci[k] * srcj[i];
						}
					}
					for (let j = i + 1; j < size; ++j) {
						srcj = src[j];
						dstj = dst[j];
						for (let k = 0; k < i; ++k) {
							dstj[k] = dstj[k] - dsti[k] * srcj[i];
						}
						dstj[i] = -diag * srcj[i];
						for (let k = i + 1; k < size; ++k) {
							srcj[k] = srcj[k] - srci[k] * srcj[i];
						}
					}
				}
				for (let i = 1; i < size; ++i) {
					dsti = dst[i];
					for (let j = 0; j < i; ++j) {
						dst[j][i] = dsti[j];
					}
				}
				break;
		}


	}

	// --- internal ---

	public computeInvMass(info: JointSolverInfo, massData: Array<JointSolverMassDataRow>): void {
		var invMass: Array<Array<number>> = this._invMass;
		var invMassWithoutCfm: Array<Array<number>> = this._invMassWithoutCfm;

		var numRows: number = info.numRows;

		var b1: RigidBody = info.b1;
		var b2: RigidBody = info.b2;

		var invM1: number;
		var invM2: number;
		invM1 = b1._invMass;
		invM2 = b2._invMass;
		var invI1: Mat3 = b1._invInertia.clone();
		var invI2: Mat3 = b2._invInertia.clone();

		// compute invM*trpJ
		//
		// | 1/m  0    0    0       0       0      | | row1  ...  rowN |
		// | 0    1/m  0    0       0       0      |
		// | 0    0    1/m  0       0       0      |
		// | 0    0    0    invI00  invI01  invI02 |
		// | 0    0    0    invI10  invI11  invI12 |
		// | 0    0    0    invI20  invI21  invI22 |
		for (let i = 0; i < numRows; ++i) {
			var j: JacobianRow = info.rows[i].jacobian;
			var md: JointSolverMassDataRow = massData[i];
			j.updateSparsity();

			if (j.isLinearSet()) {
				M.vec3_scale(md.invMLin1, j.lin1, invM1);
				M.vec3_scale(md.invMLin2, j.lin2, invM2);
			} else {
				md.invMLin1.zero();
				md.invMLin2.zero();
			}
			if (j.isAngularSet()) {
				M.vec3_mulMat3(md.invMAng1, j.ang1, invI1);
				M.vec3_mulMat3(md.invMAng2, j.ang2, invI2);
			} else {
				md.invMAng1.zero();
				md.invMAng2.zero();
			}
		}

		// compute J*(invM*trpJ)
		//
		// | row1^T | | 1/m  0    0    0       0       0      | | row1  ...  rowN |
		// | ...    | | 0    1/m  0    0       0       0      |
		// | rowN^T | | 0    0    1/m  0       0       0      |
		//            | 0    0    0    invI00  invI01  invI02 |
		//            | 0    0    0    invI10  invI11  invI12 |
		//            | 0    0    0    invI20  invI21  invI22 |
		for (let i = 0; i < numRows; ++i) {
			// rows of LHS matrix
			var j1: JacobianRow = info.rows[i].jacobian;

			for (let j = i; j < numRows; ++j) { // compute only elements in upper triangle
				// cols of RHS matrix
				var j2: JacobianRow = info.rows[j].jacobian;
				var md2: JointSolverMassDataRow = massData[j];

				var val: number =
					M.vec3_dot(j1.lin1, md2.invMLin1) +
					M.vec3_dot(j1.ang1, md2.invMAng1) +
					M.vec3_dot(j1.lin2, md2.invMLin2) +
					M.vec3_dot(j1.ang2, md2.invMAng2)
					;
				if (i == j) {
					invMass[i][j] = val + info.rows[i].cfm;
					invMassWithoutCfm[i][j] = val;

					md2.mass = val + info.rows[i].cfm;
					md2.massWithoutCfm = val;

					if (md2.mass != 0) md2.mass = 1 / md2.mass;
					if (md2.massWithoutCfm != 0) md2.massWithoutCfm = 1 / md2.massWithoutCfm;
				} else {
					invMass[i][j] = val;
					invMass[j][i] = val;
					invMassWithoutCfm[i][j] = val;
					invMassWithoutCfm[j][i] = val;
				}
			}
		}

		// clear cached submatrices
		this.clearCache();
	}


	public getSubmatrix(indices: Array<number>, n: number): Array<Array<number>> {
		var id: number = 0;
		for (let i = 0; i < n; ++i) {
			id |= 1 << indices[i];
		}
		if (this._cacheComputed[id]) {
			// the submatrix has been computed after the last change of the inverse mass matrix
			return this._cachedSubmatrices[id];
		}
		// there's no cache, compute the submatrix
		this.computeSubmatrix(id, indices, n);
		this._cacheComputed[id] = true;
		return this._cachedSubmatrices[id];
	}


	public clearCache(): void {
		for (let i = 0; i < this._maxSubmatrixId; ++i) {
			this._cacheComputed[i] = false;
		}
	}

}
